﻿using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using System;
using System.Collections.Generic;
using W3C.Soap;

//using vision = Microsoft.Robotics.Services.Sample.SimpleVision;
using System.ComponentModel;
using System.Drawing;
using System.Drawing.Imaging;
using System.Runtime.InteropServices;


namespace UrbanCam
{

    /// <summary>
    /// ColorVector for color feature description
    /// </summary>
    [DataContract]
    public class ColorVector
    {
        /// <summary>
        /// Normalized Red Component
        /// </summary>
        [Description("Indicates the normalized Red value = Red/(Red+Green+Blue).\n(Range = 0.0-1.0)")]
        [DataMember]
        public double Red;
        /// <summary>
        /// Normalized Green Component
        /// </summary>
        [Description("Indicates the normalized Green value = Green/(Red+Green+Blue).\n(Range = 0.0-1.0)")]
        [DataMember]
        public double Green;
        /// <summary>
        /// Normalized Blue Component
        /// </summary>
        [Description("Indicates the normalized Blue value = Blue/(Red+Green+Blue).\n(Range = 0.0-1.0)")]
        [DataMember]
        public double Blue;

        /// <summary>
        /// Similarity Threshold Value
        /// </summary>
        [Description("Indicates the similarity threshold value; comparing two color vectors.\n(Typical range = 0.9~1.0)")]
        [DataMember]
        public double SimilarityMeasure;

        /// <summary>
        /// Constructor
        /// </summary>
        public ColorVector()
        {
            SimilarityMeasure = 0.995;
        }
        /// <summary>
        /// Constructor with red, green
        /// </summary>
        public ColorVector(double red, double green)
        {
            Red = red;
            Green = green;
            SimilarityMeasure = 0.995;
        }
        /// <summary>
        /// Constructor with red, green, blue
        /// </summary>
        public ColorVector(double red, double green, double blue)
        {
            Red = red;
            Green = green;
            Blue = blue;
            SimilarityMeasure = 0.995;
        }
        /// <summary>
        /// Constructor with red, green, blue, similarity
        /// </summary>
        public ColorVector(double red, double green, double blue, double similarity)
        {
            Red = red;
            Green = green;
            Blue = blue;
            SimilarityMeasure = similarity;
        }
        /// <summary>
        /// Return the Magnitude of the vector
        /// </summary>
        public double Magnitude()
        {
            return Math.Sqrt(Red * Red + Green * Green + Blue * Blue);
        }
        /// <summary>
        /// Return the Dot Product of two Vectors
        /// </summary>
        public static double DotProduct(ColorVector cv1, ColorVector cv2)
        {
            if (cv1 == null)
                return 0;
            if (cv2 == null)
                return 0;

            return (cv1.Red * cv2.Red) + (cv1.Green * cv2.Green) + (cv1.Blue * cv2.Blue);
        }
        /// <summary>
        /// Calculate the similarity between two color vectors
        /// </summary>
        public static double CompareSimilarity(ColorVector cv1, ColorVector cv2)
        {
            if (cv1 == null)
                return 0;
            if (cv2 == null)
                return 0;

            double mag = (cv1.Magnitude() * cv2.Magnitude());
            if (mag != 0)
                return (DotProduct(cv1, cv2) / mag);
            return 0;
        }
    }

    public class Vison
    {
        public Vison(int width,int height)
        {
            ImageWidth = width;
            ImageHeight = height;
        }

        int ImageWidth;
        int ImageHeight;

        //public int Neighbor(byte* image, int x, int y)
        //{
        //    int neighbor = 0;

        //    // self
        //    if (image[ImageWidth * y + x] != 0) neighbor++;

        //    // 4 neighbor
        //    if (image[ImageWidth * (y - 1) + x] != 0) neighbor++;
        //    if (image[ImageWidth * (y + 1) + x] != 0) neighbor++;
        //    if (image[ImageWidth * y + x - 1] != 0) neighbor++;
        //    if (image[ImageWidth * y + x + 1] != 0) neighbor++;

        //    // 8 neighbor
        //    if (image[ImageWidth * (y - 1) + x - 1] != 0) neighbor++;
        //    if (image[ImageWidth * (y - 1) + x + 1] != 0) neighbor++;
        //    if (image[ImageWidth * (y + 1) + x - 1] != 0) neighbor++;
        //    if (image[ImageWidth * (y + 1) + x + 1] != 0) neighbor++;

        //    return neighbor;
        //}

    


        //public void Smooth(byte[] pbSrc, byte[] pbTar)
        //{
        //    int x, y;
        //    int neighbor;

        //    for (y = 1; y < ImageHeight - 1; y++)
        //    {
        //        for (x = 1; x < ImageWidth - 1; x++)
        //        {
        //            neighbor = Neighbor(pbSrc, x, y);

        //            int index = ImageWidth * y + x;
        //            if (neighbor > 4)
        //                pbTar[index] = 255;
        //            else
        //                pbTar[index] = 0;
        //        }
        //    }
        //}

        //byte[] tmpImg = null;//new byte[ImageWidth * ImageHeight];

        public void ProcessFrameHandler(
          ColorVector trackingRColor,
            ColorVector trackingGColor,
            ColorVector trackingBColor,
          int colorAreaThreshold,
            BitmapData srcData,
            BitmapData destData,
            NotifySignalState state

            )
        {

            unsafe
            {
                byte* sScan0 = (byte*)(void*)srcData.Scan0;
                byte* dScan0 = (byte*)(void*)destData.Scan0;

                int offset = 0;
                int indexCount = 0;
                for (int y = 0; y < srcData.Height; y++)
                {


                    for (int x = 0; x < srcData.Width; x++)
                    {
                        int r, g, b;
                        int indedx = (int)(y * srcData.Stride + x);

                        //scan0[offset]  = 1;
                        b = sScan0[offset++];
                        g = sScan0[offset++];
                        r = sScan0[offset++];
                        offset++;


                        if (y < 74)
                        {
                            if (IsMyColor(r, g, b, trackingRColor))
                            {
                                dScan0[indexCount] = 255;
                                state.RCount++;
                            }
                            else if (IsMyColor(r, g, b, trackingGColor))
                            {
                                dScan0[indexCount] = 255;
                                state.GCount++;
                            }
                            else
                            {
                                dScan0[indexCount] = 0;
                                //scan0[offset - 3] = 0;
                                //scan0[offset - 2] = 0;
                            }

                        }
                        else
                        {
                            if (IsMyColor(r, g, b, trackingBColor))
                            {
                                dScan0[indexCount] = 255;
                            }
                            else
                            {// 못가는 지역
                                dScan0[indexCount] = 0;
                                if (x < srcData.Width / 2)
                                    state.LeftCount++;
                                else
                                    state.RightCount++;
                            }
                        }

                        indexCount++;
                    }
                }

                //Smooth(dScan0, dScan0);
                //Smooth(tmpImg, colorImg);
                //int x, y;
                //int neighbor;

                for (int y = 1; y < ImageHeight - 1; y++)
                {
                    for (int x = 1; x < ImageWidth - 1; x++)
                    {
                        //neighbor = Neighbor(pbSrc, x, y);


                        int neighbor = 0;

                        // self
                        if (dScan0[ImageWidth * y + x] != 0) neighbor++;

                        // 4 neighbor
                        if (dScan0[ImageWidth * (y - 1) + x] != 0) neighbor++;
                        if (dScan0[ImageWidth * (y + 1) + x] != 0) neighbor++;
                        if (dScan0[ImageWidth * y + x - 1] != 0) neighbor++;
                        if (dScan0[ImageWidth * y + x + 1] != 0) neighbor++;

                        // 8 neighbor
                        if (dScan0[ImageWidth * (y - 1) + x - 1] != 0) neighbor++;
                        if (dScan0[ImageWidth * (y - 1) + x + 1] != 0) neighbor++;
                        if (dScan0[ImageWidth * (y + 1) + x - 1] != 0) neighbor++;
                        if (dScan0[ImageWidth * (y + 1) + x + 1] != 0) neighbor++;

                        int index = ImageWidth * y + x;
                        if (neighbor > 4)
                            dScan0[index] = 255;
                        else
                            dScan0[index] = 0;
                    }
                }
            }

        }

        public bool IsMyColor(int red, int green, int blue, ColorVector myColor)
        {
            double sum = red + green + blue;
            if (sum == 0) return false;

            if (Math.Abs(red - myColor.Red) > 20)
                return false;
            if (Math.Abs(green - myColor.Green) > 20)
                return false;
            if (Math.Abs(blue - myColor.Blue) > 20)
                return false;

            //ColorVector curColor = new ColorVector((double)red / sum, (double)green / sum, (double)blue / sum);
            //if (Math.Abs(ColorVector.CompareSimilarity(curColor, myColor)) > myColor.SimilarityMeasure)
            //    return true;

            return true;
        }


        public bool IsMyColor2(int red, int green, int blue, ColorVector myColor)
        {
            double sum = red + green + blue;
            if (sum == 0) return false;


            ColorVector curColor = new ColorVector((double)red / sum, (double)green / sum, (double)blue / sum);
            if (Math.Abs(ColorVector.CompareSimilarity(curColor, myColor)) > myColor.SimilarityMeasure)
                return true;

            return false;
        }


    }

      

    public class SignalData
    {
        public double _Red_R = 230;

        public double _Red_G = 0;

        public double _Red_B = 0;

        public double _Red_similarity = 0.995d;


        public double _Green_R = 78;

        public double _Green_G = 212;

        public double _Green_B = 25;

        public double _Green_similarity = 0.9d;

    }
}
